//
// Created by jm on 2021/5/28.
//

#include <planner/jps_planner/jps.h>
#include <planner/distance_map_planner/distance_map_planner.h>
#include "planner/planner.h"
#include "../example/timer.hpp"
#include "map/grid_map.h"
namespace sz{

VectorPoint2i Planner::Plan(const std::shared_ptr<const NavigationMap> &ori_map, const Point2f &start, const Point2f &goal) {
  Timer time_dilate(true);
  vec_Veci<2> dilate_neighbor;
  for (int i = -3; i < 4; ++i) {
    for (int j = -2; j < 3; ++j) {
      dilate_neighbor.emplace_back(Point2i{i, j});
    }
  }
  for (int i = -1; i < 2; ++i) {
    dilate_neighbor.emplace_back(Point2i{i, -3});
    dilate_neighbor.emplace_back(Point2i{i, 3});
  }
  auto dilate_map = ori_map->Dilate(dilate_neighbor);
  auto dt_dilate = time_dilate.Elapsed().count();
  printf("Map dilate takes: %ld ms\n\n", dt_dilate);

  // Set up sz planner
  JPS jps(false); // Declare a planner
  jps.setMapUtil(dilate_map); // Set collision checking function


  // Run sz planner

  VectorPoint2i path_jps;
  Timer time_jps(true);
  jps.Plan(start, goal, 1);
  auto dt_jps = time_jps.Elapsed().count();
  path_jps = jps.GetMapRawPath(); // Get the planned raw path from sz
  printf("JPS Planner takes: %ld ms\n", dt_jps);
  printf("JPS Path Distance: %f\n\n", total_distance2i(path_jps));

  // Set up DMP planner
  DMPlanner dmp(false);
  dmp.SetPotentialRadius(Point2f(.2, .2)); // Set 2D potential field radius
  dmp.SetSearchRadius(Point2f(.4, .4)); // Set the valid search region around given path
  dmp.SetPow(1);
  dmp.SetMap(dilate_map, start); // Set zslam_map zslam_util for collision checking, must be called before planning

  // Run DMP planner
  Timer time_dist(true);
  dmp.Plan(start, goal, path_jps); // Compute the path given the jps path
  const auto path_dist = dmp.GetMapRawPath();
  auto dt_dist = time_dist.Elapsed().count();
  printf("DMP Planner takes: %ld ms\n", dt_dist);
  printf("DMP Path Distance: %f\n\n", total_distance2i(path_dist));
  return path_dist;
}
}
